# Get current pose 
import urx
import math3d as m3d
import math
from math import pi, sin, cos
import time 
import sys
import logging


def ouler_to_quaternion(rx, ry, rz):
    c1 = cos(rx/2)
    c2 = cos(rz/2)
    c3 = cos(ry/2)
    s1 = sin(rx/2)
    s2 = sin(rz/2)
    s3 = sin(ry/2)
    c1c2 = c1*c2
    s1s2 = s1*s2
    w = c1c2*c3 - s1s2*s3;
    x = c1c2*s3 + s1s2*c3;
    y = s1*c2*c3 + c1*s2*s3;
    z = c1*s2*c3 - s1*c2*s3;
    quaternion = m3d.UnitQuaternion(w, x, y, z)
    return quaternion

if __name__ == "__main__":
    logging.basicConfig(level = logging.INFO)
    robot = urx.Robot("192.168.0.108")
    robot.set_tcp((-0.038890873, -0.038890873, 0.22, 0, 0, -math.pi/4.0))
    robot.set_payload(0.5, (0, 0, 0))

    try:
        # Get joints
        joints = robot.getj()
        print("\n")
        print("The current joints: ")
        print(joints)

        # Get pose (x, y, z, rx, ry, rz)
        print("\n")
        pose = robot.getl()
        print("The current pose: ")
        print(pose)

        # 
        # print("\npose in quaternion:")
        # print(ouler_to_quaternion(pose[3], pose[4], pose[5]))

        # get_pose
        print("\n")
        print("get current transform from base to to tcp: ")
        print(robot.get_pose())


    finally:
        robot.close()
